//
// Created by sukai on 22-9-19.
//

#include <iostream>
#include <ros/ros.h>
#include <signal.h>
#include "contnav_msgs/Dictionaries.h"
#include "contnav_msgs/DictionariesVector.h"
#include "contnav_srvs/TABLE_CONTORLLER.h"
#include "common.h"


#include <algorithm>

using namespace std;
/**
 *
 * @param argc
 * @param argv
 * @return
 * 测试节点
 * 2022-09-19
 * sukai
 * rosrun contnav testParse_node
 */



map<string, string> dictionariesMap_;




//ctrl + c
void mySigintHandler(int sig)
{
    std::cout << "ctrl c  sig: "<<sig<<std::endl;

    ros::shutdown();
    if (ros::isShuttingDown()){
        cout<<"close ros  test.cpp"<<endl;
    }
}



//去掉所有空格
std::string get_string(std::string res){
    int r = res.find(' ');
    while (r != string::npos){
        if (r != string::npos)
        {
           res.replace(r, 1, "");
          r = res.find(' ');
        }
      }
    return res;
}

//分割
std::vector<std::string>  split_data(std::string pattern,std::string str)
{

    std::vector<std::string> result;
    try {

        std::string::size_type pos;

        pattern+=str; //扩展字符串以方便操作
        int  size=pattern.size();

        for ( int  i=0; i<size; i++)
        {
            pos=pattern.find(str,i);
            if (pos<size)
            {
                std::string s=pattern.substr(i,pos-i);
                if (s.size()>0){
                    result.push_back(s);
                }
                i=pos+str.size()-1;
            }
        }


    }catch (std::exception e){
        //qDebug() << "CarStartServuceClient::split 抛出异常！";
    }
    return result;
}


//解析数据
void parseData(string &msg) {
    try {

        if (!msg.empty()) {

            cout<<"1msg: |"<<msg<<"||"<<endl;

            msg = get_string(msg);//去掉所有空格及换行符

            cout<<"2msg: |"<<msg<<"||"<<endl;
            //分割
            std::vector<std::string> msglist0 = split_data(msg, "?");
            cout<<"msglist0.size(): "<<msglist0.size()<<endl;
            cout<<"msglist0[0]: "<<msglist0[0]<<endl;
            cout<<"msglist0[1]: "<<msglist0[1]<<endl;

            if (msglist0.size() > 1 && "/api/move" == msglist0[0]) { //机器人移动功能 【单机】


                std::vector<std::string>  list0 = split_data(msglist0[1],"&");

                string markers="";
                string tasks="";
                string messages="";

                for (int j = 0; j < list0.size(); ++j) {
                  string value =  list0[j];
                  std::vector<std::string>  list0 = split_data(value,"=");

                      if(list0.size()>1&&list0[0]=="marker")
                         markers=list0[1];

                      if(list0.size()>1&&list0[0]=="task")
                          tasks=list0[1];

                      if(list0.size()>1&&list0[0]=="message")
                          messages=list0[1];
                }

                    if(markers.size()==0)
                    return;

                    std::vector<std::string>  marker_vector = split_data(markers,",");
                    std::vector<std::string>  task_vector;
                    std::vector<std::string>  message_vector;

                    if(tasks.size()>0 )
                    task_vector = split_data(tasks,",");

                    if(messages.size()>0)
                     message_vector = split_data(messages,",");


                    //1. 得到 marker
                for (int k = 0; k < marker_vector.size(); ++k) {
                    cout<<"marker_vector: "<<marker_vector[k]<<endl;
                }
                 // 2.得到 task
                 if(task_vector.size()==marker_vector.size()){
                    for (int k = 0; k < task_vector.size(); ++k) {
                        cout<<"task_vector: "<<task_vector[k]<<endl;
                    }

                  }
                //3. 得到 message
                if(message_vector.size()==marker_vector.size()){
                        for (int k = 0; k < message_vector.size(); ++k) {
                            cout<<"message_vector: "<<message_vector[k]<<endl;
                        }

                    }

/**
                marker_vector: target_name
                marker_vector: target_name1
                marker_vector: target_name2
                task_vector: task_name
                task_vector: task_name1
                task_vector: task_name2
                message_vector: message
                message_vector: message1
                message_vector: message2

*/

            }
        }
    }catch (exception e){
        cout<<"解析数据 异常"<<endl;
    }

}



// rosrun contnav testParse_node
int main(int argc,char *argv[]){
    //节点名
    string nodeName = "testParse_contnav_contnav_global_planner_node";
    //初始化节点
    ros::init(argc,argv,nodeName);
    //创建节点
    ros::NodeHandle node;
    //监控 ctrl c
    signal(SIGINT, mySigintHandler);

    string msg="  /api/move? marker=target_name,target_name1,target_name2&task=task_name,task_name1,task_name2&message=message,message1,message2";
    parseData(msg);


    //事件轮询
    ros::spin();
    return 0;
}



